Motion estimating device

ABSTRACT

A motion estimating device first detects mobile objects Oi and Oi′ in continuous image frames T and T′, and acquires image areas Ri and Ri′ corresponding to the mobile objects Oi and Oi′. Then, the motion estimating device removes the image areas Ri and Ri′ corresponding to the mobile objects Oi and Oi′ in the image frames T and T′, extracts corresponding point pairs Pj of feature points between the image frames T and T′ from the image areas having removed the image areas Ri and Ri′, and carries out the motion estimation of the autonomous mobile machine between the image frames T and T′ on the basis of the positional relationship of the corresponding point pairs Pj of feature points.

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present invention relates to a motion estimating device thatestimates the motion state of an autonomous mobile machine, such as anautomatic driving vehicle, a mobile robot, or the like.

2. Related Background Art

As a motion estimating device which estimates the motion state of anautonomous mobile machine, for example, Japanese Patent UnexaminedApplication Publication (Translation of PCT Application) No. 2008-503757describes a motion estimating device which acquires an image sequence ofan ambient environment by using a video camera, processes the imagesequence in accordance with, for example, video processing technique,and performs motion estimation with respect to the ambient environment.

SUMMARY OF THE INVENTION

Like the related art, when motion estimation is performed by using theimages of the ambient environment, for example, the motion state of theautonomous mobile machine is estimated on the basis of the correspondingfeature points extracted from the respective images. In this case,however, the corresponding feature points of the respective images areassumed to be present on stationary objects, so when the extractedcorresponding feature points are present on mobile objects (vehicles,pedestrians, or the like), it may be impossible to correctly performmotion estimation of the autonomous mobile machine.

It is an object of the invention to provide a motion estimating devicewhich can accurately perform motion estimation of an autonomous mobilemachine even if a mobile object exists in an image.

An aspect of the invention provides a motion estimating device whichcaptures images of an ambient environment of an autonomous mobilemachine by using an imaging device, and estimates the motion state ofthe autonomous mobile machine on the basis of change in the respectivecaptured images of the ambient environment. The motion estimating deviceincludes an image area removing section detecting mobile objects whichexist in the respective captured images of the ambient environment, andremoving image areas corresponding to the mobile objects, acorresponding feature point extracting section extracting correspondingfeature points from the respective captured images having removed theimage areas corresponding to the mobile objects, and a first estimatingsection estimating the motion state of the autonomous mobile machine onthe basis of the positional relationship between the correspondingfeature points of the respective captured images.

With this motion estimating device, the mobile objects which exist inthe respective captured images of the ambient environment of theautonomous mobile machine acquired by the imaging device are detected,and the image areas corresponding to the mobile objects are removed.Then, the corresponding feature points are extracted from the respectivecaptured images having removed the image areas corresponding to themobile objects. Accordingly, there is no inconsistency that thecorresponding feature points which actually exist on the mobile objectsare extracted as being on the stationary objects in the captured images.Therefore, even if a mobile object exists in each captured image, themotion estimation of the autonomous mobile machine can be accuratelyperformed.

The motion estimating device according to the aspect of the inventionmay further include a determining section determining whether or not thenumber of corresponding feature points is larger than a predeterminedvalue. When the determining section determines that the number ofcorresponding feature points is larger than the predetermined value, thefirst estimating section may estimate the motion state of the autonomousmobile machine on the basis of the positional relationship between thecorresponding feature points of the respective captured images. In thiscase, a sufficient number of corresponding feature points are used forthe motion estimation, and as a result, the motion estimation of theautonomous mobile machine can be further accurately performed.

The motion estimating device according to the aspect of the inventionmay further include a second estimating section estimating the motionstate of the autonomous mobile machine on the basis of the previouspositions of the autonomous mobile machine when the determining sectiondetermines that the number of corresponding feature points is not largerthan the predetermined value. In this case, even if there are a smallnumber of stationary objects which exist in the respective capturedimage, the motion estimation of the autonomous mobile machine can beperformed.

The motion estimating device according to the aspect of the inventionmay further include a position detecting section acquiring the positionof the mobile object with respect to the imaging device, a speeddetecting section acquiring the speed of the mobile object, a temporaryestimating section estimating the temporary motion state of theautonomous mobile machine on the basis of the previous positions of theautonomous mobile machine when the determining section determines thatthe number of corresponding feature points is not larger than thepredetermined value, and a second estimating section estimating themotion state of the autonomous mobile machine on the basis of theposition of the mobile object with respect to the imaging device, thespeed of the mobile object, and the temporary motion state of theautonomous mobile machine. In this case, even if there are a smallnumber of stationary objects which exist in each captured image, themotion estimation of the autonomous mobile machine can be performedinsofar as there are a necessary number of mobile objects in eachcaptured image.

According to the aspect of the invention, even if a mobile object existsin an image, the motion estimation of the autonomous mobile machine canbe accurately performed. Therefore, for example, the subsequent motioncontrol of the autonomous mobile machine can be easily performed.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a block diagram showing the schematic configuration of anembodiment of a motion estimating device according to the invention.

FIG. 2 is a flowchart showing the details of the procedure of a motionestimation processing which is carried out by a motion estimatingsection shown in FIG. 1.

FIG. 3 is a schematic view showing an example of image frames which areobtained by an image processing section shown in FIG. 1.

FIG. 4 is a schematic view showing a state where image areascorresponding to mobile objects are removed from the image frames shownin FIG. 3.

FIG. 5 is a diagram schematically showing an operation when the motionestimating device shown in FIG. 1 estimates the motion state of anautonomous mobile machine.

FIG. 6 is a flowchart showing another procedure of the motion estimationprocessing which is carried out by the motion estimating section shownin FIG. 1.

DESCRIPTION OF THE PREFERRED EMBODIMENTS

Hereinafter, a preferred embodiment of a motion estimating deviceaccording to the invention will be described in detail with reference tothe drawings.

FIG. 1 is a block diagram showing the schematic configuration of anembodiment of a motion estimating device according to the invention. Amotion estimating device 1 of this embodiment is a device which ismounted on an autonomous mobile machine, such as an automatic drivingvehicle, a mobile robot, or the like, and estimates the state ofself-motion (movement, rotation, or the like) of the autonomous mobilemachine.

The motion estimating device 1 includes a camera 2 capturing images ofthe ambient environment of the autonomous mobile machine, and an ECU(Electronic Control Unit) 3 having a CPU, a memory, such as a ROM, aRAM, or the like, an input/output circuit, and the like. The singlecamera 2 or two or more cameras 2 may be provided.

The ECU 3 has an image processing section 4, a storage section 5, amotion estimating section 6, and a driving control section 7. The imageprocessing section 4 performs image processing, such as filterprocessing, binarization processing, feature extraction processing, andthe like, on the captured images of the ambient environment of theautonomous mobile machine acquired by the camera 2, and generates imageframes.

The storage section 5 stores in advance data (mobile object data)regarding the shapes or postures of mobile objects, such as vehicles,for example, automobiles or the like, two-wheeled vehicles, bicycles,pedestrians, and the like, which move independently from the autonomousmobile machine. As mobile object data, a lot of data is registered foreach type of mobile object.

The motion estimating section 6 receives the image frames obtained bythe image processing section 4 as input, performs a predeterminedprocessing on the image frames by using mobile object data stored in thestorage section 5, and estimates how the autonomous mobile machine ismoving.

The driving control section 7 controls the driving system of theautonomous mobile machine (in an automatic driving vehicle, includingthe steering system) in accordance with the motion state estimated bythe motion estimating section 6.

FIG. 2 is a flowchart showing the details of the procedure of a motionestimation processing which is carried out by the motion estimatingsection 6.

Referring to FIG. 2, first, a latest image frame T′ and a previous imageframe T are prepared, mobile objects Oi and Oi′ which exist in the imageframes T and T′ are detected, and image areas Ri and Ri′ correspondingto the mobile objects Oi and Oi′ are acquired (Step S101). Note that i=1to N (N is the number of detected mobile objects).

Specifically, pattern recognition is performed by comparing mobileobject candidates, which exist in the image frames T and T′, with mobileobject data stored in the storage section 5, and when the similaritybetween the mobile object candidates and any mobile object data is equalto or larger than a predetermined value, the relevant mobile objectcandidates are set as the mobile objects Oi and Oi′.

FIG. 3 shows an example of the image frames T and T′. In the imageframes T and T′, there are image areas R1 and R1′ corresponding tomobile objects (pedestrians) O1 and O1′ and image areas R2 and R2′corresponding to mobile objects (vehicles) O2 and O2′. Further, in theimage frames T and T′, there are three stationary objects (buildings andthe like).

Next, the positions Pi(Xi,Yi,Zi) and Pi′(Xi′,Yi′,Zi′) of the mobileobjects Oi and Oi′ when viewed from the camera 2 are calculated by usinga known method (Step S102).

The movement distance of each mobile object is acquired from theposition coordinates of the mobile object when viewed from the camera 2,and the speed Vi of the mobile object is calculated on the basis of themovement distance and the acquisition time interval (for example, 100ms) of the captured images (Step S103). In this case, in order toincrease the calculation accuracy of the speed Vi, the speed Vi of themobile object is desirably calculated by using information regarding thelatest two image frames T and T′ and a large number of past imageframes.

Next, the image areas Ri and Ri′ corresponding to the mobile objects Oiand Oi′ in the image frames T and T′ are removed (Step S104). Forexample, if this processing is performed on the image frames T and T′shown in FIG. 3, as shown in FIG. 4, images are obtained in which theimage areas R1 and R1′ corresponding to the mobile objects O1 and O1′and the image areas R2 and R2′ corresponding to the mobile objects O2and O2′ are removed, and the three stationary objects remain.

Next, a corresponding point pair Pj of feature points (where j=1 to M: Mis the number of corresponding point pairs) between the image frames Tand T′ is acquired from the image areas having removed the image areasRi and Ri′ corresponding to the mobile objects Oi and Oi′ (Step S105).The feature points are points which represent the characteristicportions of the stationary objects remaining in the image frames T andT′.

Next, it is determined whether or not the number M of correspondingpoint pairs of feature points (corresponding feature points) is largerthan a prescribed threshold value (Step S106). When it is determinedthat the number M of corresponding point pairs Pj of feature points islarger than the threshold value, the motion estimation of the autonomousmobile machine between the image frames T and T′ is performed on thebasis of the positional relationship of the corresponding point pair Pjof feature points (Step S107). In this case, as the method for themotion estimation, a factorization method (for example, see C. Tomasiand Kanade, Shape and Motion from Image Streams under Orthography: AFactorization Method, International Journal of Computer Vision, pp.137-154, 1992) or the like is used.

When it is determined that the number M of corresponding point pairs Pjof feature points is not larger than the threshold value, the motionestimation of the autonomous mobile machine between the image frames Tand T′ is performed by using the past motion estimation result of theautonomous mobile machine (Step S108). In this case, the motionestimation is performed, for example, by extrapolation of the pastmovements of the autonomous mobile machine.

In Step S108, the motion estimation of the autonomous mobile machine maybe performed by using the positional relationship between thecorresponding point pair Pj of feature points in the image frames T andT′ as well as the past motion estimation result of the autonomous mobilemachine.

Steps S101 and S104 of the storage section 5 and the motion estimatingsection 6 in the ECU 3 constitute an image area removing section whichdetects mobile objects which exist in the respective captured images ofthe ambient environment, and removes image areas corresponding to themobile objects. Step S105 of the motion estimating section 6 constitutesa corresponding feature point extracting section which extractscorresponding feature points from the respective captured images havingremoved the image areas corresponding to the mobile objects. Step S107of motion estimating section 6 constitutes a first estimating sectionwhich estimates the motion state of the autonomous mobile machine on thebasis of the positional relationship between the corresponding featurepoints of the respective captured images.

Step S106 of the motion estimating section 6 constitutes a determiningsection which determines whether or not the number of correspondingfeature points is larger than a predetermined value. Step S108 of themotion estimating section 6 constitutes a second estimating sectionwhich, when the determining section determines that the number ofcorresponding feature points is not larger than the predetermined value,estimates the motion state of the autonomous mobile machine on the basisof the past positions of the autonomous mobile machine.

FIG. 5 schematically shows an operation when the motion estimatingdevice 1 of this embodiment estimates the motion state of the autonomousmobile machine. Here, as shown in FIG. 5( a), it is assumed that knownobjects A and B are present in front of the traveling autonomous mobilemachine. In this case, the known objects A and B are imaged by thecamera 2, so an image frame T with the known objects A and B isacquired.

When the known objects A and B are stationary objects, and a sufficientnumber of feature points are present on the stationary objects, as shownin FIG. 5( b), due to the traveling of the autonomous mobile machine,the camera 2 approaches the stationary objects. Then, the stationaryobjects at that time are imaged by the camera 2 to acquire an imageframe T′, and corresponding point pairs of feature points between theimage frames T and T′ are obtained. Therefore, the motion state of theautonomous mobile machine can be accurately estimated.

When the motion estimation is performed assuming that the known objectsA and B, which are actually mobile objects, are stationary objects, asshown in FIG. 5( c), even if the autonomous mobile machine is actuallygoing forward, it is erroneously estimated that the autonomous mobilemachine is stopped or going backwards.

In contrast, in this embodiment, when the known object A and B aremobile objects, this situation can be recognized. Therefore, the mobileobjects after being moved are imaged by the camera 2 to acquire theimage frame T′, and the past motion estimation result of the autonomousmobile machine is used, thereby estimating the motion state of theautonomous mobile machine.

As described above, according to this embodiment, the mobile objects Oiand Oi′ in the continuous image frames T and T′ are detected, the imageareas Ri and Ri′ corresponding to the mobile objects Oi and Oi′ areremoved, the corresponding point pairs Pj of feature points in theresultant image frames T and T′ are extracted, and when the number ofcorresponding point pairs Pj is larger than the threshold value, themotion state of the autonomous mobile machine is estimated on the basisof the positional relationship of the corresponding point pairs Pj offeature points between the image frames T and T′. In this way, when themobile objects exist in the image frames, the image areas correspondingto the mobile objects are removed from the image frames. Thus, as shownin FIG. 5( c), the motion estimation is prevented from being performedassuming that the mobile objects are stationary objects. In this case,even if the mobile object is stopped or the change in the speed of themobile object is close to zero, since the image area corresponding tothe mobile object is removed, an estimation error due to the subtlemovement of a mobile object can be eliminated. Therefore, even ifmultiple mobile objects exist in the vicinity of the autonomous mobilemachine, the motion estimation of the autonomous mobile machine can beperformed with high accuracy.

When the number of corresponding point pairs Pj of feature points issmaller than the threshold value, the past motion estimation result ofthe autonomous mobile machine is used for the motion estimation. Thus,even if a small number of stationary object exist in the image frame,the motion state of the autonomous mobile machine can be estimated.

When the motion estimation is performed by using the GPS (GlobalPositioning System), the motion estimation may become difficult sinceelectric waves from the GPS satellite are blocked by buildings or thelike. Further, when the motion estimation is performed by detecting therotation speed of the wheels, the motion estimation may become difficultdue to skidding. In contrast, according to the motion estimating device1 of this embodiment, such inconsistency can be avoided.

FIG. 6 is a flowchart showing another procedure of the motion estimationprocessing which is carried out by the motion estimating section 6. Inthe drawing, the same steps as those in the flowchart of FIG. 2 arerepresented by the same reference numerals, and descriptions thereofwill be omitted.

In FIG. 6, when it is determined in Step S106 that the number M ofcorresponding point pairs Pj of feature points is not larger than thethreshold value, temporary motion estimation of the autonomous mobilemachine between the image frames T and T′ is carried out by using thepast motion estimation result of the autonomous mobile machine (StepS111).

Next, the position Pi′(Xi′,Yi′,Zi′) of the mobile object Oi′ at the timeof the image frame T′ when viewed from the camera 2 is corrected byusing the position Pi of the mobile object Oi and the speed Vi of themobile object (Step S112). In this case, correction calculation iscarried out assuming that the mobile object is moving uniformly at thespeed Vi.

Then, the motion estimation of the autonomous mobile machine between theimage frames T and T′ is carried out on the basis of the positionalrelationship between the position Pi of the mobile object Oi and thecorrected position Pi′ of the mobile object Oi′, the speed Vi of themobile object, and the temporary motion estimation result of theautonomous mobile machine (Step S113). In this case, as the motionestimation method, for example, the factorization method or the like maybe used, similarly to the above-described step S107.

Note that in Step S113, the motion estimation of the autonomous mobilemachine may be carried out by using the positional relationship betweenthe corresponding point pairs Pj of feature points in the image frames Tand T′.

In the above description, Step S102 of the motion estimating section 6constitutes a position detecting section which acquires the position ofthe mobile object with respect to the imaging device 2. Step S103 of themotion estimating section 6 constitutes a speed detecting section whichacquires the speed of the mobile object. Step S111 of the motionestimating section 6 constitutes a temporary estimating section which,when the determining section determines that the number of correspondingfeature points is not larger than a predetermined value, estimates thetemporary motion state of the autonomous mobile machine on the basis ofthe past positions of the autonomous mobile machine. Steps S112 and S113of the motion estimating section 6 constitute a second estimatingsection which estimates the motion state of the autonomous mobilemachine on the basis of the position of the mobile object with respectto the imaging device 2, the speed of the mobile object, and thetemporary motion state of the autonomous mobile machine.

In such a configuration, when the known objects A and B are mobileobjects, this situation can be recognized. Therefore, as shown in FIG.5( d), the mobile object after being moved is imaged by the camera 2 toacquire the image frame T′, the positional relationship of the mobileobject between the image frames T and T′ and the speed of the mobileobject are acquired, and the movement of the mobile object is predicted,thereby accurately estimating the motion state of the autonomous mobilemachine.

The invention is not limited to the foregoing embodiment. For example,while in the foregoing embodiment, the speed Vi of the mobile object isacquired, and when the number of corresponding point pairs of featurepoints on the stationary object is smaller than the threshold value, themotion estimation is carried out assuming that the mobile object ismoving uniformly at the speed Vi, the motion estimation may be carriedout taking into consideration a change in the speed of the mobileobject, such as acceleration or a jerk, without assuming the mobileobject is moving uniformly.

1. A motion estimating device that captures images of an ambientenvironment of an autonomous mobile machine by using an imaging device,and estimates a motion state of the autonomous mobile machine on thebasis of a change in respective captured images of the ambientenvironment, the motion estimating device comprising: an image arearemoving section detecting mobile objects which exist in the respectivecaptured images of the ambient environment, and removing image areascorresponding to the mobile objects; a corresponding feature pointextracting section extracting corresponding feature points from therespective captured images having removed the image areas correspondingto the mobile objects; and a first estimating section estimating themotion state of the autonomous mobile machine on the basis of apositional relationship between the corresponding feature points of therespective captured images.
 2. The motion estimating device according toclaim 1, further comprising: a determining section determining whetheror not a number of corresponding feature points is larger than apredetermined value, wherein, when the determining section determinesthat the number of corresponding feature points is larger than thepredetermined value, the first estimating section estimates the motionstate of the autonomous mobile machine on the basis of the positionalrelationship between the corresponding feature points of the respectivecaptured images.
 3. The motion estimating device according to claim 2,further comprising: a second estimating section estimating the motionstate of the autonomous mobile machine on the basis of previouspositions of the autonomous mobile machine when the determining sectiondetermines that the number of corresponding feature points is not largerthan the predetermined value.
 4. The motion estimating device accordingto claim 2, further comprising: a position detecting section acquiring aposition of the mobile object with respect to the imaging device; aspeed detecting section acquiring a speed of the mobile object; atemporary estimating section estimating a temporary motion state of theautonomous mobile machine on the basis of previous positions of theautonomous mobile machine when the determining section determines thatthe number of corresponding feature points is not larger than thepredetermined value; and a second estimating section estimating themotion state of the autonomous mobile machine on the basis of theposition of the mobile object with respect to the imaging device, thespeed of the mobile object, and the temporary motion state of theautonomous mobile machine.